#include <functional>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

#include <general_interface/action/auto_task.hpp>
#include <general_interface/msg/chassis_servo.hpp>
#include <general_interface/msg/control_signal.hpp>
#include <general_interface/srv/mode_selection.hpp>

/*
 *  used for testing
 *  when at servo mode,core do nothing
 *  when at manual mode,core will publish chassis_servo from control_signal,and stop all runing task
 */
class Core : public rclcpp::Node
{
  private:
    rclcpp::Publisher<general_interface::msg::ChassisServo>::SharedPtr chassis_pub;
    rclcpp::Subscription<general_interface::msg::ControlSignal>::SharedPtr control_sub;
    rclcpp::Service<general_interface::srv::ModeSelection>::SharedPtr mode_client;
    rclcpp_action::Client<general_interface::action::AutoTask>::SharedPtr connect_task_client;
    rclcpp_action::Client<general_interface::action::AutoTask>::SharedPtr disconnect_task_client;
    bool isManualMode = true;
    bool isServoMode = false;
    double forward_scale = 1.0, rotate_scale = 1.0;

  public:
    explicit Core() : Node("Core")
    {
        declare_parameter<bool>("servo_mode", false);
        isServoMode = get_parameter("servo_mode").as_bool();

        declare_parameter<double>("forward_scale", 1.0);
        declare_parameter<double>("rotate_scale", 1.0);
        forward_scale = get_parameter("forward_scale").as_double();
        rotate_scale = get_parameter("rotate_scale").as_double();

        chassis_pub = this->create_publisher<general_interface::msg::ChassisServo>("chassis_servo", 1);
        control_sub = this->create_subscription<general_interface::msg::ControlSignal>(
            "control_signal", 1, std::bind(&Core::on_control_signal, this, std::placeholders::_1));
        mode_client = this->create_service<general_interface::srv::ModeSelection>(
            "auto_mode", std::bind(&Core::on_mode_selection, this, std::placeholders::_1, std::placeholders::_2,
                                   std::placeholders::_3));
        connect_task_client = rclcpp_action::create_client<general_interface::action::AutoTask>(this, "connect_task");
        disconnect_task_client =
            rclcpp_action::create_client<general_interface::action::AutoTask>(this, "disconnect_task");
    }

  private:
    void on_control_signal(const general_interface::msg::ControlSignal::SharedPtr msg)
    {
        if (!isManualMode || isServoMode)
        {
            return;
        }
        general_interface::msg::ChassisServo chassis_msg;
        chassis_msg.speed = msg->forward_control * forward_scale;
        chassis_msg.angle = 0;
        chassis_msg.gyro = 0;
        chassis_msg.gyroz = msg->rotate_control * rotate_scale;
        chassis_msg.speed_gain = 1;
        chassis_msg.angle_gain = 0;
        chassis_msg.gyro_gain = 0;
        chassis_msg.gyroz_gain = 1;
        chassis_pub->publish(chassis_msg);
    }

    void on_mode_selection(const std::shared_ptr<rmw_request_id_t> request_header,
                           const std::shared_ptr<general_interface::srv::ModeSelection::Request> request,
                           const std::shared_ptr<general_interface::srv::ModeSelection::Response> response)
    {
        (void)request_header;
        request->mode == general_interface::srv::ModeSelection::Request::MANUAL ? isManualMode = true
                                                                                : isManualMode = false;
        response->success = true;
        RCLCPP_INFO(this->get_logger(), "Mode changed to %s",
                    request->mode == general_interface::srv::ModeSelection::Request::MANUAL ? "MANUAL" : "AUTO");
        return;
    }

    void InvokeConnectTask()
    {
        auto goal_msg = general_interface::action::AutoTask::Goal();
        goal_msg.task_id = general_interface::action::AutoTask::Goal::CONNECT;
        rclcpp_action::Client<general_interface::action::AutoTask>::SendGoalOptions goal_options;
        // goal_options.goal_response_callback = std::bind(&Core::on_task_response, this, std::placeholders::_1);
        connect_task_client->async_send_goal(goal_msg, goal_options);
    }

    void InvokeDisconnectTask()
    {
        auto goal_msg = general_interface::action::AutoTask::Goal();
        goal_msg.task_id = general_interface::action::AutoTask::Goal::DISCONNECT;
        rclcpp_action::Client<general_interface::action::AutoTask>::SendGoalOptions goal_options;
        // goal_options.goal_response_callback = std::bind(&Core::on_task_response, this, std::placeholders::_1);
        disconnect_task_client->async_send_goal(goal_msg, goal_options);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Core>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}